/* io_arm7_jtag.c -- JTAG transport for ARM7TDMI, using jtag_protocol
**
** Copyright 2006, Brian Swetland.  All rights reserved.
** See provided LICENSE file or http://frotz.net/LICENSE for details.
*/

#include <string.h>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>

#include "../library/arm7.h"
#include "../jtag/jtag_protocol.h"

#include "debugger.h"
#include "usb.h"

#define THUMB_BX_PC 0x47784778
#define THUMB_NOP   0x46C046C0

static unsigned state[16];

static int
jtag_exec(unsigned *cmd, unsigned *rep, unsigned count, unsigned rcount)
{
    unsigned header[2];
    
    if(count > 1024) {
        IOERROR("jtag_exec() transaction too large");
        return -1;
    }
    
    header[0] = JTAG_COMMAND;
    header[1] = count;

    if(usb_write(header, 8) != 8) {
        IOERROR("jtag_exec() failed to write header");
        return -1;
    }

    count *= 4;
    if(usb_write(cmd, count) != count){
        IOERROR("jtag_exec() failed to write command");
        return -1;
    }

    if(usb_read(header, 8) != 8){
        IOERROR("jtag_exec() failed to read header");
        return -1;
    }

    if(header[0] == JTAG_ERROR){
        IOERROR("jtag_exec() received error response (%d)", header[1]);
        return -1;
    }

    if(header[0] != JTAG_REPLY){
        IOERROR("jtag_exec() received unknown response");
        return -1;
    }

    count = header[1];
    if(count != rcount) {
        IOERROR("jtag_exec() response size mismatch");
        return -1;
    }

    if(count) {
        count *= 4;
        if(usb_read(rep, count) != count){
            IOERROR("jtag_exec() failed to read response");
            return -1;
        }
    }
    return count / 4;
}

static void
jtag_reset(void)
{
    unsigned n = OP_RESET;
    jtag_exec(&n, &n, 1, 0);
}



static void
save_state(void)
{
    unsigned buf[18];
    
    buf[0] = OP_DBG_WR | COUNT(3);
    
    buf[1] = ARM_STMIA_LR(0xffff);
    buf[2] = ARM_NOP;
    buf[3] = ARM_NOP;

    buf[4] = OP_DBG_RD | COUNT(17);

    jtag_exec(buf, buf, 5, 17);

    memcpy(state, buf, 16 * 4);
    
}

void
dump_state(void)
{
    msg("r0  %08x  r1  %08x  r2  %08x  r3  %08x",
        state[0], state[1], state[2], state[3]);
    msg("r4  %08x  r5  %08x  r6  %08x  r7  %08x",
        state[4], state[5], state[6], state[7]);
    msg("r8  %08x  r9  %08x  r10 %08x  r11 %08x",
        state[8], state[9], state[10], state[11]);
    msg("r12 %08x  sp  %08x  lr  %08x  pc  %08x",
        state[12], state[13], state[14], state[15]);
}

static int
jtag_halt_cpu(int reset)
{
    int r;
    unsigned buf[32];

    jtag_reset();
    
    if(reset) {
        buf[0] = OP_RESET_0;
        jtag_exec(buf, buf, 1, 0);
    } else {
        buf[0] = OP_ICE_RD;
        buf[1] = ICE_DEBUG_STATUS;
        jtag_exec(buf, buf, 2, 1);
        
        if(buf[0] & 1) {
            save_state();
            return 0;
        }
    }

    buf[0] = OP_ICE_WR | COUNT(7);
    buf[1] = ICE_WATCH0_ADDR_VALUE;
    buf[2] = 0x00000000;
    buf[3] = ICE_WATCH0_ADDR_MASK;
    buf[4] = 0xffffffff;
    buf[5] = ICE_WATCH0_DATA_VALUE;
    buf[6] = 0x00000000;
    buf[7] = ICE_WATCH0_DATA_MASK;
    buf[8] = 0xffffffff;
    buf[9] = ICE_WATCH0_CTRL_VALUE;
    buf[10] = 0x00000000;
    buf[11] = ICE_WATCH0_CTRL_MASK;
    buf[12] = 0xfffffff7;
    buf[13] = ICE_WATCH0_CTRL_VALUE;
    buf[14] = 0x00000100;
    buf[15] = OP_RESET_1;
    buf[16] = OP_ICE_WAIT;
    buf[17] = ICE_DEBUG_STATUS;
    buf[18] = 9;
    buf[19] = 9;
    buf[20] = OP_ICE_WR | COUNT(1);
    buf[21] = ICE_WATCH0_CTRL_VALUE;
    buf[22] = 0;
    buf[23] = OP_RESET;
    buf[24] = OP_ICE_RD;
    buf[25] = ICE_DEBUG_STATUS;
    
    if(jtag_exec(buf, buf, 26, 1) != 1){
        r = -1;
    } else {
        r = 0;
        if(buf[0] & ICE_DEBUG_TBIT){
            msg("moving to arm mode");
            buf[0] = OP_DBG_WR | COUNT(3);
            buf[1] = THUMB_BX_PC;
            buf[2] = THUMB_NOP;
            buf[3] = THUMB_NOP;
            jtag_exec(buf, buf, 4, 0);
        }

        save_state();
    }

    if(reset){
        buf[0] = OP_RESET_1;
        jtag_exec(buf, buf, 1, 0);
    }
    return r;
}

static unsigned
get_id(void)
{
    unsigned n = OP_ID_RD;
    jtag_exec(&n, &n, 1, 1);
    return n;
}

static unsigned
jtag_rd_reg(unsigned n)
{
    unsigned buf[5];
    
    buf[0] = OP_DBG_WR | COUNT(3);
    buf[1] = ARM_STR_Rx_R14(n);
    buf[2] = ARM_NOP;
    buf[3] = ARM_NOP;
    buf[4] = OP_DBG_RD | COUNT(1);

    jtag_exec(buf, buf, 5, 1);
    return buf[0];
}

static void
jtag_wr_reg(unsigned n, unsigned v)
{
    unsigned buf[9];

    buf[1] = ARM_MOV_Rx_0(14);
    buf[2] = ARM_LDR_Rx_R14(n);
    buf[3] = ARM_NOP;
    buf[4] = ARM_NOP;
    buf[5] = v;
    buf[6] = ARM_NOP;

    if(n == 15){
        buf[0] = OP_DBG_WR | COUNT(8);
        buf[7] = ARM_NOP;
        buf[8] = ARM_NOP;
        jtag_exec(buf, buf, 9, 0);
    } else {
        buf[0] = OP_DBG_WR | COUNT(6);
        jtag_exec(buf, buf, 7, 0);
    }
}

static void
jtag_jump_to(DebugIO *io, unsigned addr)
{
    unsigned buf[30];

    msg("resuming at %08x\n", addr);
    state[15] = addr;

    buf[0] = OP_DBG_WR | COUNT(23);
    buf[1] = ARM_MOV_Rx_0(14);
    buf[2] = ARM_LDMIA_LR(0xffff);
    buf[3] = ARM_NOP;
    buf[4] = ARM_NOP;

    memcpy(buf + 5, state, 16 * 4);
    
    buf[21] = ARM_NOP;
    buf[22] = ARM_NOP;
    buf[23] = ARM_NOP;
    
    buf[24] = OP_DBG_WR_BKPT;
    buf[25] = OP_DBG_WR | COUNT(1);
    buf[26] = 0xEAFFFFFB;
    buf[27] = OP_RESTART;
    buf[28] = OP_RESET;
    jtag_exec(buf, buf, 29, 0);
}

static unsigned
jtag_rd32(DebugIO *io, unsigned addr)
{
    unsigned buf[15];

        /* load address into r0 */
    buf[0] = OP_DBG_WR | COUNT(6);
    
    buf[1] = ARM_MOV_Rx_0(14);
    buf[2] = ARM_LDR_Rx_R14(0);
    buf[3] = ARM_NOP;
    buf[4] = ARM_NOP;
    buf[5] = addr;
    buf[6] = ARM_NOP;

    buf[7] = OP_EXEC;
    buf[8] = ARM_LDR_R1_R0;    

        /* read the value of r1 */
    buf[9] = OP_DBG_WR | COUNT(3);
    buf[10] = ARM_STR_Rx_R14(1);
    buf[11] = ARM_NOP;
    buf[12] = ARM_NOP;
    buf[13] = OP_DBG_RD | COUNT(1);
    buf[14] = ARM_NOP;
    
    jtag_exec(buf, buf, 15, 1);

    return buf[0];
}

static void
jtag_wr32(DebugIO *io, unsigned addr, unsigned data)
{
    unsigned buf[10];
    
    buf[0] = OP_DBG_WR | COUNT(7);

        /* make sure the base register is aligned */
    buf[1] = ARM_MOV_Rx_0(14);

        /* load addr,data into r0,r1 */
    buf[2] = ARM_LDMIA_LR(3);
    buf[3] = ARM_NOP;
    buf[4] = ARM_NOP;
    buf[5] = addr;
    buf[6] = data;
    buf[7] = ARM_NOP;

    buf[8] = OP_EXEC;
    buf[9] = ARM_STR_R1_R0;
    
    jtag_exec(buf, buf, 10, 0);
}

static unsigned regbits[15] = {
    0x0000,
    0x0001, 0x0003, 0x0007, 0x000f,
    0x001f, 0x003f, 0x007f, 0x00ff,
    0x01ff, 0x03ff, 0x07ff, 0x0fff,
    0x1fff, 0x3fff,
};

static void
jtag_wr32x(DebugIO *io, unsigned addr, unsigned *data, unsigned count)
{
    unsigned buf[1024];
    unsigned i, n;

    jtag_wr_reg(14, addr);
    
    n = 0;
    
    while(count > 0){
        unsigned xfer = (count > 14) ? 14 : count;
        unsigned mask = regbits[xfer];
        
        buf[n++] = OP_DBG_WR | COUNT(4 + xfer);

        buf[n++] = ARM_LDMIA_LR(mask);
        buf[n++] = ARM_NOP;
        buf[n++] = ARM_NOP;
        for(i = 0; i < xfer; i++) {
            buf[n++] = *data++;
        }
        buf[n++] = ARM_NOP;

        buf[n++] = OP_EXEC;
        buf[n++] = ARM_STMIA_LR_BANG(mask);

        if(n > 768){
            jtag_exec(buf, buf, n, 0);
            n = 0;
        }
        count -= xfer;
    }
    if(n > 0) {
        jtag_exec(buf, buf, n, 0);
    }
}

void
cmd_reset()  // JTAG: reset cpu and force into debug mode
{
    msg("forcing CPU to reset/halt");

    jtag_reset();
    
    jtag_halt_cpu(0);

        /* XXX: should be per-target not in this file */
        /* enable the reset line */
    wr32(0xfffffd08, 0xa5000401);
    
    jtag_halt_cpu(1);
}

void
cmd_status() // JTAG: query the debug status register
{
    unsigned buf[2];
    buf[0] = OP_ICE_RD;
    buf[1] = ICE_DEBUG_STATUS;
    jtag_exec(buf, buf, 2, 1);
    
    msg("ICE Status %08x\n", buf[0]);
}

void
cmd_reboot() // JTAG: strobe the RESET line
{
    unsigned buf[9];
    buf[0] = OP_RESET_0;
    buf[1] = OP_RESET_0;
    buf[2] = OP_RESET_0;
    buf[3] = OP_RESET_0;
    buf[4] = OP_RESET_0;
    buf[5] = OP_RESET_0;
    buf[6] = OP_RESET_0;
    buf[7] = OP_RESET_0;
    buf[8] = OP_RESET_1;
    jtag_exec(buf, buf, 9, 0);
}


unsigned jtag_dbg_status()
{
    unsigned buf[2];

    buf[0] = OP_ICE_RD;
    buf[1] = ICE_COMMS_CONTROL;
    jtag_exec(buf, buf, 2, 1);
    
    return buf[0];
}

unsigned jtag_dbg_read()
{
    unsigned buf[2];

    buf[0] = OP_ICE_RD;
    buf[1] = ICE_COMMS_DATA;
    jtag_exec(buf, buf, 2, 1);

    return buf[0];
}

void
cmd_jreset() // reset the JTAG state machine
{
    jtag_reset();
}

static void
jtag_connect(DebugIO *io)
{
    if(io->online) return;

    if(usb_open(0x4242, 0x0002, 0, 0x81, 0x03)){
        return;
    }

    io->online = 1;
    jtag_reset();
}


static void
jtag_disconnect(DebugIO *io)
{
    if(io->online) {
        usb_close();
        io->online = 0;
    }
}

static void
jtag_halt()
{
    jtag_halt_cpu(0);
    dump_state();
}

static void jtag_debug_poll(DebugIO *io);

extern Command tbl_io_arm7_jtag[];

DebugIO DebugIO_ARM7_JTAG =
{
    ._rd32 = jtag_rd32,
    ._wr32 = jtag_wr32,
    ._rd32x = default_rd32x,
    ._wr32x = jtag_wr32x,
    ._jump_to = jtag_jump_to,
    ._halt = jtag_halt,
    .connect = jtag_connect,
    .disconnect = jtag_disconnect,
    .poll = jtag_debug_poll,
    .commands = tbl_io_arm7_jtag,
    
    .name = "JTAG",
};

#define MAXDBG 128
static char dbgbuf[MAXDBG];
static int dbgcount = 0;

static void
jtag_debug_poll(DebugIO *io)
{
    unsigned n;
    io->error = 0;
    

    for(;;) {
        if(io->online == 0) return;

        n = jtag_dbg_status();
        if(failed()) return;

            /* this reserved bit is usually 0
               if it's 1, we're probably not
               connected and should ignore the DCC
            */
        if(n & 4) return;
        
        if((n & 2) == 0) return;
        
        n = jtag_dbg_read();
        if(failed()) return;

        dbgbuf[dbgcount++] = n;
        if((n == 10) || (dbgcount == MAXDBG)){
            dbgbuf[dbgcount-1] = 0;
            msg("dbg: %s\n",dbgbuf);
            dbgcount = 0;
            return;
        }
    }
}

